/* CORBAServer.cc -- Servidor CORBA
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part of GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include <iostream>

#include "CORBAServer.hh"

using namespace std;

// Objeto Visao_impl
void
Visao_impl::setField( const float * field ) throw (CORBA::SystemException)
{
    vector<float> field_vec;

    for( int i = 0 ; i < LENGTH_FIELD_VECTOR ; i++ )
	field_vec.push_back(field[i]);
	
    monitor_->setField(field_vec);
}

void
Visao_impl::setWorld( const float * world ) throw (CORBA::SystemException)
{
    vector<float> world_vec;

    for( int i = 0 ; i < LENGTH_WORLD_VECTOR ; i++ )
	world_vec.push_back(world[i]);
	
    monitor_->setWorld(world_vec);
}

void
Visao_impl::setMonitor( Monitor * monitor )
{
    monitor_ = monitor;
}
///////////////////////////////////

// Objeto IA_impl
void
IA_impl::SetVel( const CORBA::Short id, const float vx, const float vy, const float va )  throw (CORBA::SystemException)
{
    control_->robotSetVel( id, vx, vy, va );
}

void
IA_impl::DribbleEnable( const CORBA::Short id, const CORBA::Short enable ) throw (CORBA::SystemException)
{
    control_->robotDribbleEnable( id, enable);
}

void
IA_impl::Kick( const CORBA::Short id, const CORBA::Short nivel ) throw (CORBA::SystemException)
{
    control_->robotKick( id, nivel );
}

void
IA_impl::SayHello( const CORBA::Short id ) throw (CORBA::SystemException)
{
    xerror.Info("O robô %d diz \"OLA!\"", id );
}

void
IA_impl::getField( float * field ) throw (CORBA::SystemException)
{
    vector<float> field_vec;

    monitor_->getField( field_vec );

    for( int i = 0 ; i < LENGTH_FIELD_VECTOR ; i++ )
	field[i] = field_vec.at(i);
}

void
IA_impl::getWorld( float * world ) throw (CORBA::SystemException)
{
    vector<float> world_vec;

    monitor_->getWorld( world_vec );

    for( int i = 0 ; i < LENGTH_WORLD_VECTOR ; i++ )
	world[i] = world_vec.at(i);
}

void
IA_impl::setMonitor( Monitor * monitor )
{
    monitor_ = monitor;
}

void
IA_impl::setControl( Control * control )
{
    control_ = control;
}

///////////////////////////////////


CORBAServer::CORBAServer()
{
    orb = NULL;
}

CORBAServer::~CORBAServer()
{
    if( orb )
	orb->shutdown(1);
}

void
CORBAServer::Init( int argc, char * argv[], Monitor * monitor_ptr, Control * control_ptr )
{
    try
    {
	orb = CORBA::ORB_init(argc, argv);
	
	CORBA::Object_var obj
	    = orb->resolve_initial_references("RootPOA");
	PortableServer::POA_var poa
	    = PortableServer::POA::_narrow(obj);

	// Objeto Visao
	visao_servant_ = new Visao_impl();	
	PortableServer::ObjectId_var visaoid =
	    poa->activate_object(visao_servant_);
	obj = visao_servant_->_this();
	CosNaming::Name objectName;
	objectName.length(1);
	objectName[0].id = (const char*) "Visao";
	objectName[0].kind = (const char*) "gm";
	if( !bindObjectToName(orb, obj, objectName) )
	    return; 
	visao_servant_->_remove_ref();

	// Objeto IA
	ia_servant_ = new IA_impl();
	PortableServer::ObjectId_var iaid =
	    poa->activate_object(ia_servant_);
	obj = ia_servant_->_this();
	objectName[0].id = (const char*) "IA";
	objectName[0].kind = (const char*) "gm";
	if( !bindObjectToName(orb, obj, objectName) )
	    return; 
	ia_servant_->_remove_ref();
	
	PortableServer::POAManager_var mgr
	    = poa->the_POAManager();
	mgr->activate();
    }
    catch (const CORBA::Exception &)
    {
	xerror.Error("Uncaught CORBA exception:");
    }

    // Adicionando ponteiro para Monitor
    monitor = monitor_ptr;
    visao_servant_->setMonitor( monitor_ptr );
    ia_servant_->setMonitor( monitor_ptr );

    // Adicionando ponteiro para Control
    control_ = control_ptr;
    ia_servant_->setControl( control_ptr );
    
    return;    
}

void
CORBAServer::Run(void * ptr)
{
    xerror.Info("Servidor pronto e esperando.");
    orb->run();
}

CORBA::Boolean
CORBAServer::bindObjectToName(CORBA::ORB_ptr orb, CORBA::Object_ptr objref,
			      CosNaming::Name objectName )
{
    CosNaming::NamingContext_var rootContext;
    try {
        // Obtain a reference to the root context of the Name service:
	CORBA::Object_var obj;
	obj = orb->resolve_initial_references("NameService");
	// Narrow the reference returned.
	rootContext = CosNaming::NamingContext::_narrow(obj);
	if( CORBA::is_nil(rootContext) ) {
	    xerror.Error( "Failed to narrow the root naming context.");
	    return 0;
	}
    }  catch (CORBA::NO_RESOURCES&) {
	    xerror.Error( "Caught NO_RESOURCES exception. You must configure omniORB with the location of the naming service.");
	return 0;
    }  catch (CORBA::ORB::InvalidName&) {
	// This should not happen!
	    xerror.Error( "Service required is invalid [does not exist].");
	return 0;
    }

    try {
	try {
	    rootContext->bind(objectName, objref);
	}  catch(CosNaming::NamingContext::AlreadyBound& ex)
	{
	    rootContext->rebind(objectName, objref);
	}
    }  catch(CORBA::TRANSIENT& ex) {
	xerror.Error( "Caught system exception TRANSIENT -- unable to contact the "
		      "naming service."
		      "Make sure the naming server is running and that omniORB is "
		      "configured correctly." );
	return 0;
    }  catch(CORBA::SystemException& ex) {
	    xerror.Error( "Caught a CORBA:: %s while using the naming service.", ex._name());
	return 0;
    }
    return 1;
}
